/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "RSTController.h"

#include "Polynomial.h"

#include "Utils.h"

#include <sstream>
#include <numeric>

#include "Adaptive/StaticAlgorithm.h"
#include "Adaptive/APZP1Algorithm.h"
#include "Adaptive/APPAlgorithm.h"

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

const std::string RSTController::CONF_ADPTTYPE = "adptype";
const std::string RSTController::CONF_ADPTINDEX = "adptindex";
const std::string RSTController::CONF_ADPSETTINGS = "adpt::";


RSTController::RSTController(SimInterface::ISimulator &isim)
    : ControllerBase(isim, "RST", "RSTController")
{
    m_adpt.reset(new StaticAlgorithm(*this));

    m_strToAdpt["static"] = ISimulator::AdptTypeStatic;
    m_strToAdpt["apzp1"] = ISimulator::AdptTypeAPZP1;
    m_strToAdpt["app"] = ISimulator::AdptTypeAPP;

    resizeBuffersIfNecessary();
}

void RSTController::resizeBuffersIfNecessary()
{
    unsigned r = m_adpt->getRRank(), // w umem nie przechowujemy u(i), dlatego jeden mniej
        s = m_adpt->getSRank() + 1,
        t = m_adpt->getTRank() + 1;

    if(r > m_umem.size())
        m_umem.resize(r);

    if(s > m_ymem.size())
        m_ymem.resize(s);

    if(t > m_wmem.size())
        m_wmem.resize(t);
}

void RSTController::createAlgorithm(AdptType adptType)
{
    switch(adptType)
    {
    case ISimulator::AdptTypeStatic:
        m_adpt.reset(new StaticAlgorithm(*this));
        break;

    case ISimulator::AdptTypeAPZP1:
        m_adpt.reset(new APZP1Algorithm(*this));
        break;

    case ISimulator::AdptTypeAPP:
        m_adpt.reset(new APPAlgorithm(*this));
        break;

    default:
         throw logic_error("Unsupported type of adaptive algorithm.");
    }
}

double RSTController::simulateControllerStep(double setPoint, double processVar)
{
    // Najpierw wkładamy próbki w(i) i y(i) do pamięci gdyż potrzebujemy wartości bierzących.
    // setPoint = w(i)
    // processVar = y(i)
    m_wmem.push_front(setPoint);
    m_wmem.pop_back();             // usunięcie ostatniej próbki.
    m_ymem.push_front(processVar);
    m_ymem.pop_back();             // usunięcie ostatniej próbki.

    // m_umem odświeżamy na końcu, tzn. że:
    // w tej chwili zawiera w pierwszej pozycji element u(i-1) !!!

    // Podsumowując: w tej chwili bufory zaczynają się od:
    // m_wmem - w(i)
    // m_ymem - y(i)
    // m_umem - u(i-1)

    // Definicja iteratorów:
    vector<double>::iterator
            rpoly_it = ++m_adpt->rbegin(),                   // zaczynamy od r1 nie r0
            rpoly_last_it = m_adpt->rend(),
            spoly_it = m_adpt->sbegin(),                     // tu zaczynamy od s0
            spoly_last_it = m_adpt->send(),
            tpoly_it = m_adpt->tbegin(),                     // tu zaczynamy od t0
            tpoly_last_it = m_adpt->tend();
    deque<double>::iterator
            umem_it = m_umem.begin(),                        // zaczynamy od u(i-1)
            wmem_it = m_wmem.begin(),                        // zaczynamy od w(i)
            ymem_it = m_ymem.begin();                        // zaczynamy od y(i)

    // Obliczenie sumy częściowej: -r1*u(i-1)-r2*u(i-2)-...-rdR*u(i-dR)
    double tmpSum = 0;
    tmpSum = inner_product(rpoly_it, rpoly_last_it, umem_it, tmpSum, minus<double>(), multiplies<double>());

    // Dodanie do sumy wyrazu: t0*w(i)+t1*w(i-1)+...+tdT*w(i-dT)
    tmpSum = inner_product(tpoly_it, tpoly_last_it, wmem_it, tmpSum, plus<double>(), multiplies<double>());

    // Dodanie do sumy wyrazu: -s0*y(i)-s1*y(i-1)-...-sdS*y(i-dS)
    tmpSum = inner_product(spoly_it, spoly_last_it, ymem_it, tmpSum, minus<double>(), multiplies<double>());

    // tmpSum w tej chwili reprezentuje próbkę sterowania u(i)

    // Zapamietanie wyjścia u(i)
    m_umem.push_front(tmpSum);
    m_umem.pop_back();

    // TMP
    // Ograniczenie
    tmpSum = Utils::limitValue(tmpSum, 0.0, 1.0);

    // Dokonujemy kroku adaptacji.
    m_adpt->adapt(tmpSum, processVar, setPoint);

    return tmpSum;
}

void RSTController::reset()
{
    fill(m_umem.begin(), m_umem.end(), 0);
    fill(m_wmem.begin(), m_wmem.end(), 0);
    fill(m_ymem.begin(), m_ymem.end(), 0);
    m_adpt->reset();
}

//
// Sprawy konfiguracyjne
//

/**
  Zwraca pod opcję, np. z adpt::cośtam zwraca cośtam. Jeśli początek nie spełnia
  określonej reguły to zwraca false.
*/
static bool getSubOption(const std::string& src, const std::string& pattern, std::string& opt)
{
    if(src.length() < pattern.length())
        return false;

    if(src.substr(0, pattern.length()) == pattern)
    {
        opt = src.substr(pattern.length());
        return true;
    }
    else
    {
        return false;
    }
}

bool RSTController::setParameter(const std::string& param, const std::string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);
    string tmpSubstr;

    double tmpVal;
    istr >> tmpVal;

    if(istr.bad())
        throw iconf_error::syntaxError(*this, tvalue);

    if(tparam == CONF_ADPTTYPE)
    {
        if(Utils::mapContains(m_strToAdpt, tvalue))
        {
            createAlgorithm(m_strToAdpt[tvalue]);
        }
        else
            throw iconf_error::unrecognizedParam(*this, tvalue);
    }
    else if(getSubOption(tparam, CONF_ADPSETTINGS, /*out*/ tmpSubstr))
    {
        // Przekaż podobcję składowej klasy:
        return m_adpt->setParameter(tmpSubstr, tvalue);
    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    return true;
}

std::string RSTController::getParameter(const std::string& param) const throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;
    string tmpSubstr;

    // Strumień danych dla łańcucha argumentu.
    ostringstream ostr(outStr);

    // Wiem, sieczka straszna:
    if(tparam == CONF_ADPTTYPE)
    {
        ostr << m_adpt->getType();
    }
    else if(tparam == CONF_ADPTINDEX)
    {
        int idx = m_strToAdpt.find(m_adpt->getType())->second;
        ostr << idx;
    }
    else if(getSubOption(tparam, CONF_ADPSETTINGS, /*out*/ tmpSubstr))
    {
        ostr << m_adpt->getParameter(tmpSubstr);
    }
    else
        return ControllerBase::getParameter(tparam);

    return ostr.str();
}

SimFile::ConfigWriter& RSTController::dumpSpecificPar(SimFile::ConfigWriter& o) const
{
    o.pushParam(CONF_ADPTTYPE, m_adpt->getType());
    return m_adpt->dumpParameters(o);
}

}
}
